#include "board.hpp"

using namespace hw;

volatile int64_t current_time = 0;
static uint32_t systick_load;

#define RCC_PLL_N 0x150
#define RCC_PLL_Q 0x7

void Board::init() {
  RCC->CR |= ((uint32_t)RCC_CR_HSEON);
  while (!(RCC->CR & RCC_CR_HSERDY))
    ;

  FLASH->ACR |= FLASH_ACR_PRFTEN;
  FLASH->ACR &= (uint32_t)((uint32_t)~FLASH_ACR_LATENCY);
  FLASH->ACR |= (uint32_t)FLASH_ACR_LATENCY_5WS; // 5 WS (6 CPU cycles)
  FLASH->ACR |= FLASH_ACR_ICEN | FLASH_ACR_DCEN;

  RCC->CFGR |= RCC_CFGR_PPRE2_DIV2;
  RCC->CFGR |= RCC_CFGR_PPRE1_DIV4;
  RCC->PLLCFGR = RCC_PLLCFGR_PLLM_3 | (RCC_PLL_N << RCC_PLLCFGR_PLLN_Pos) |
                 RCC_PLLCFGR_PLLSRC_HSE | (RCC_PLL_Q << RCC_PLLCFGR_PLLQ_Pos);

  RCC->CFGR |= (RCC_CFGR_PPRE1_0 | RCC_CFGR_PPRE1_2);

  RCC->CR |= RCC_CR_PLLON;

  while ((RCC->CR & RCC_CR_PLLRDY) == 0) {
  };
  RCC->CFGR &= (uint32_t)((uint32_t) ~(RCC_CFGR_SW));
  RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL;
  while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)0x08)
    ;

  SystemCoreClockUpdate();
  SysTick_Config(SystemCoreClock / 1000);
  systick_load = SysTick->LOAD;

  // LED
  RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN;
  GPIOD->MODER |= GPIO_MODER_MODER12_0;

  // System configuration controller clock enable
  RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
}

void Board::sensor_pin_input() {
  GPIOD->MODER &= ~GPIO_MODER_MODER1_0; // input mode
  GPIOD->PUPDR = 0;

  SYSCFG->EXTICR[0] &= ~SYSCFG_EXTICR1_EXTI1; // map gpio to exti
  SYSCFG->EXTICR[0] |= (0b0011 << 4);         // map gpio to exti
  EXTI->IMR |= (1 << 1);                      // unmask line
  EXTI->FTSR |= EXTI_FTSR_TR1;                // falling edge
  EXTI->RTSR |= EXTI_RTSR_TR1;                // rising edge
  NVIC_EnableIRQ(EXTI1_IRQn);                 // enable interrupt
}

void Board::sensor_pin_output(int value) {
  EXTI->IMR &= ~(1 << 1);              // mask line / disable exti
  EXTI->PR |= EXTI_PR_PR1;             // clear interrupt request flag
  GPIOD->MODER |= GPIO_MODER_MODER1_0; // output mode
  GPIOD->OTYPER &= ~GPIO_OTYPER_OT1;   // pullup/pull-down
  GPIOD->PUPDR &= ~GPIO_PUPDR_PUPD1;   // push-pull
  GPIOD->PUPDR |= GPIO_PUPDR_PUPD1_0;  // pull-up
  GPIOD->OSPEEDR |= GPIO_OSPEEDR_OSPEED1_0;
  if (value) {
    GPIOD->ODR |= GPIO_ODR_OD1; // 1 on output
  } else {
    GPIOD->ODR &= ~GPIO_ODR_OD1; // 0 on output
  }
}

rl::TimePoint Board::get_now() {
    auto ms = current_time;
    auto us = systick_load - SysTick->VAL;
    return ms + us;
}

void Board::enable_uart() {
  // USART PC6, PC7

  // init
  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // enable GPIOC
  RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;  // enable DMA
  GPIOC->MODER &= ~(GPIO_MODER_MODER6 | GPIO_MODER_MODER7);
  GPIOC->MODER |= (GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1); // AF function
  GPIOC->AFR[0] |=
      (GPIO_AFRL_AFSEL6_3 |
       GPIO_AFRL_AFSEL7_3); // AF8 see datasheet page 64 table 9 (1000)
  GPIOC->OSPEEDR |=
      (GPIO_OSPEEDR_OSPEED6_1 | GPIO_OSPEEDR_OSPEED7_1);     // High speed
  GPIOC->OTYPER &= ~(GPIO_OTYPER_OT6 | GPIO_OTYPER_OT7);     // Push-pull
  GPIOC->PUPDR |= (GPIO_PUPDR_PUPD6_0 | GPIO_PUPDR_PUPD7_0); // Pull-up
  RCC->APB2ENR |= RCC_APB2ENR_USART6EN; // USART Clock enable

  // setup
  static constexpr uint32_t boud_rate = 512000;
  static constexpr auto x = (8 * (2 - 0) * boud_rate);
  static constexpr auto clock_speed = (RCC_MAX_FREQUENCY / 2);
  static constexpr auto mantissa = clock_speed / x;
  static constexpr auto rem = clock_speed % x;
  static constexpr auto fraction = (rem * 16) / x;
  static constexpr auto brr = (mantissa << 4) | fraction;

  USART6->CR1 = 0; // Reset value;
  USART6->CR1 |= USART_CR1_TE;
  USART6->CR1 &= ~USART_CR1_OVER8; // oversampling 16
  USART6->CR1 &= ~USART_CR1_M;     // 8 Data bits
  USART6->CR1 &= ~USART_CR1_PCE;   // Parity control
  USART6->BRR = brr;
  USART6->CR2 &= ~USART_CR2_STOP; // 1 Stop bit
  USART6->CR3 |= USART_CR3_DMAT;  // enable DMA transfer

  USART6->CR1 |= USART_CR1_UE;
  USART6->SR &= ~USART_SR_TC;
}

void Board::uart_write(const void *ptr, uint32_t sz) {
  DMA2_Stream6->CR = 0; // Reset value
  while (DMA2_Stream6->CR & (1 << 0))
    ;
  DMA2_Stream6->CR |=
      (DMA_SxCR_CHSEL_0 |
       DMA_SxCR_CHSEL_2);            // Channel5 see RM0090 page 308, table 43
  DMA2_Stream6->CR |= DMA_SxCR_TCIE; // TC interrupt enabled
  DMA2_Stream6->CR |= DMA_SxCR_MINC;
  DMA2_Stream6->CR |= DMA_SxCR_PL_0; // Priority level medium
  DMA2_Stream6->CR |=
      DMA_SxCR_DIR_0; // Direction Memory-to-peripheral
                      //    DMA2_Stream6 ->CR &= ~(DMA_SxCR_MSIZE_0 |
                      //    DMA_SxCR_MSIZE_1);
  DMA2_Stream6->CR |= DMA_SxCR_MSIZE_1;
  //    DMA2_Stream7 ->CR |= DMA_SxCR_PSIZE_1;

  DMA2_Stream6->M0AR = (uint32_t)(ptr);
  DMA2_Stream6->PAR = (uint32_t) & (USART6->DR);
  DMA2_Stream6->NDTR = sz;

  NVIC_EnableIRQ(DMA2_Stream6_IRQn);
  DMA2_Stream6->CR |= DMA_SxCR_EN;
}

extern "C" {

void EXTI1_IRQHandler(void) {
  EXTI->PR |= EXTI_PR_PR1; // clear interrupt request flag
  exti1_irq_handler_backend();
}

void SysTick_Handler(void) { current_time += 1000; }

void DMA2_Stream6_IRQHandler(void) {
  DMA2->HIFCR |= DMA_HIFCR_CTCIF6;
  dma2_stream6_handler_backend();
}
}
